/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    dynamicModel.h
  * @author  Mentos Seetoo
  * @brief   This file provides dynamic data structure containing parameters for 
  *          quadruped robot using floating base model of MIT Cheetah Code. 
  * 		 Kinematics/Invers Kinematics solutions are provided, too.			
  * @note    THe following third-party libraries are used:
  *          - STL
  *          - Eigen3
  * @warning 
  *     - At least C++11 is required.														 
  *     - This file refers to Quadruped.cpp' in MIT Cheetah Control code. 
  * 	- More info about frame definition plz view the document paper or webside:
  * 		http://www.scut-robotlab.cn
  ******************************************************************************
  * @attention
  * 
  * if you had modified this file, please make sure there is no any bug in your
  * code. Update the version Number and write dowm your name and the date.What 
  * most important is that make sure the users will have a clear understanding
  * through your description.
  ******************************************************************************
  */
#ifndef FLOATING_BASE_MODEL_H
#define FLOATING_BASE_MODEL_H
/* Includes ------------------------------------------------------------------*/
#include "Dynamics/actuator.h"
#include "Dynamics/floatingBaseModel.h"

/* Types define --------------------------------------------------------------*/
namespace Quadruped
{
/**
 * @brief Quadruped robot specific dynamic types
 */

template <typename T>
struct ConstParam
{
    T BODY_LENGTH, BODY_WIDTH, BODY_HEIGHT, BODY_MASS;       //!< Body shape parameters. 
    T ABAD_LINK_LEN, HIP_LINK_LEN, KNEE_LINK_LEN, KNEE_Y_OFFSET, MAX_LEG_LEN;
    T ABAD_GEAR_RATIO, HIP_GEAR_RATIO, KNEE_GEAR_RATIO;      //!< Robot dynamic parameters.
    T MOTOR_MAX_TORQUE, MOTOR_KT, MOTOR_R, SUPPLY_VOLTAGE;   //!< THe max output torque is not included gear reduction.
    T JOINT_DAMP, JOINT_FRICTION;                            //!< Assume they are fixed but not necessary.
};

/**
 * @brief Dynamic model data struct. The basical name rule is ：xObject_B2A/xObject_C
 *        - 1) A is superscript of left, B is subscript of left. C is relative frame.
 *             More info about frame definition plz view the document paper.
 *        - 2) xObject: x is object symble, which indicates 
 *             whether this object is for position or velocity and so on.
 */
template <typename T>
struct LegData
{
    //EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    LegData(){zero();}

    void zero()
    {
        p = Vec3<T>::Zero();
        v = Vec3<T>::Zero();
        a = Vec3<T>::Zero();
        q = Vec3<T>::Zero();
        qd = Vec3<T>::Zero();
        tau = Vec3<T>::Zero();
        J_B = Mat3<T>::Ones();
    }

    Vec3<T> p,v,a;        //!< Position & velocity & acceleration of foot in {B}.
    Vec3<T> q, qd, tau;   //!< Angle & angle velocity & tau of joint
    Mat3<T> J_B;          //!< Jacobian.
};

template <typename T>
struct DynamicState
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Vec4<T> contactEstimate;
    Vec3<T> position;
    Vec3<T> vBody;
    Quat<T> orientation;
    RotMat<T> rBody_M2B;
    Vec3<T> rpy;

    DynamicState(){zero();}
    void zero()
    {
        contactEstimate = Vec4<T>::Zero();
        position = Vec3<T>::Zero();
        vBody = Vec3<T>::Zero();
        orientation = Quat<T>::Zero();
        rBody_M2B = RotMat<T>::Ones();
        rpy = Vec3<T>::Zero();
    }
};
/* Exported ------------------------------------------------------------------*/
namespace CONST_DEF
{
constexpr size_t DOF_NUM = 12; //!< All dof is actuated.
constexpr size_t LEG_NUM = 4;
constexpr size_t LEG_DOF_NUM = 3;
constexpr int    LEFT_SIDE = 1;
constexpr int    RIGHT_SIDE = -1;
} // namespace CONST_DEF

using std::vector;

template <typename T>
class Dynamic
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW                           //!< To use fixed-size eigen object.
    Vec3<T> _abadLocation, _abadRotorLocation, _hipLocation, _hipRotorLocation,
     _kneeLocation, _kneeRotorLocation;
    SpatialInertia<T> _abadInertia, _hipInertia, _kneeInertia, _abadRotorInertia,
    _hipRotorInertia, _kneeRotorInertia, _bodyInertia;        //!< Inertias of different parts.
    bool buildModel();                                        //!< Build model automatically.
    bool buildActuatorModels();
                                                              //!< Kinematics/Invers Kinematics.
    bool convertToCartesianSpace(const Vec3<T>& pLeg, Vec3<T> &pFoot_B);
    bool calFootPositions();
    bool convertToJointAngle(const Vec3<T>& pFoot_B, Vec3<T> &pLeg, int leg);
    bool convertToJointAngle();                   

    std::vector<ActuatorModel<T>> actuatorModels; 
    FloatingBaseModel<T> Model;
    LegData<T>      LegState[4];
    LegData<T>      LegCommand[4];
    DynamicState<T> BodyState;   
    ConstParam<T> Param;

    /*!
      * Get location of the hip for the given leg in robot frame
      * @param leg : the leg index
      */
    Vec3<T> getHipLocation(int leg)
    {
      assert(leg >= 0 && leg < 4);
      Vec3<T> pHip((leg == 0 || leg == 1) ? _abadLocation(0) : -_abadLocation(0),
                    (leg == 1 || leg == 2) ? _abadLocation(1) : -_abadLocation(1),
                    _abadLocation(2));
      return pHip;
    }
private:
    bool paramIsValid();
};

/**
 * @brief  Convert matrix v according by leg sign.
 * @param v 
 * @param legID 
 * @return Vec3<T> 
 */
template <typename T, typename T2>
Vec3<T> convertWithLegSigns(const Eigen::MatrixBase<T2> &v, int legID)
{
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 3,
                "Must have 3x1 matrix");
  switch (legID) {
    case 0:
      return Vec3<T>(v[0], -v[1], v[2]);
    case 1:
      return Vec3<T>(v[0], v[1], v[2]);
    case 2:
      return Vec3<T>(-v[0], v[1], v[2]);
    case 3:
      return Vec3<T>(-v[0], -v[1], v[2]);
    default:
      throw std::runtime_error("Invalid leg id!");
  }
}

} // namespace Quadruped

int getLegSign(int legID);
#endif //FLOATING_BASE_MODEL_H
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/